#include "GameMode1Scene.h"
#include <fstream>

#if OGRE_PLATFORM == OGRE_PLATFORM_WIN32
#include "../res/resource.h"
#endif

GameMode1Scene*	GameMode1Scene::m_singleton = NULL;

#define BIT(x) (1<<(x))
enum collisiontypes {
	COL_NOTHING = 0, //<Collide with nothing
	COL_BOUND = BIT(0),
	COL_AIRBOX = BIT(1),
	COL_BODY = BIT(2),
	COL_OBSTACLE = BIT(3)
};

const int boundCollidesWith = COL_AIRBOX;
const int airboxCollidesWith = COL_BOUND | COL_BODY | COL_OBSTACLE;
const int bodyCollidesWith = COL_AIRBOX;
const int obstacleCollidesWith = COL_AIRBOX | COL_OBSTACLE;

std::set<btRigidBody*> toBeDeleted;

bool myContactProcessedCallback(btManifoldPoint& cp, void* body0,void* body1)
{
	btRigidBody* man = (btRigidBody*) body0;
	btRigidBody* box = (btRigidBody*) body1;
	PapaNode* nodeInfo = static_cast<PapaNode*>(box->getUserPointer());
	PapaNode* nodeInfo1 = static_cast<PapaNode*>(man->getUserPointer());
	if (nodeInfo==NULL || nodeInfo1==NULL)
	{
		return false;
	}
	if (nodeInfo->type != "man" && nodeInfo1->type != "man")
	{
		return false;
	}
	if (nodeInfo->type == "man")
	{
		man = (btRigidBody*) body1;
		box = (btRigidBody*) body0;
	}
	toBeDeleted.insert(box);

	//collision can be such types:
	//box-man
	//box-obstacle
	//box-bounding
	std::vector<CollisionPair> *collisionPair = GameMode1Scene::getInstance().getCollisionPair();
	int num = collisionPair->size();
	CollisionPair pair(man, box);
	for (int i=0;i<num;i++)
	{
		if (pair == (*collisionPair)[i])
		{
			return false;
		}
	}
	collisionPair->push_back(pair);
	return false;
}

void GameMode1Scene::initPhysics()
{
	m_collisionConfiguration = new btDefaultCollisionConfiguration();
	m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
	m_overlappingPairCache = new btDbvtBroadphase();
	btSequentialImpulseConstraintSolver* sol = new btSequentialImpulseConstraintSolver;
	m_solver = sol;
	m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher, m_overlappingPairCache, m_solver, m_collisionConfiguration);
	m_dynamicsWorld->setGravity(btVector3(0, 0, 0));
	mNumEntitiesInstanced = 0;
	gContactProcessedCallback = myContactProcessedCallback;
}

void GameMode1Scene::exitPhysics()
{
	for (int i=m_dynamicsWorld->getNumCollisionObjects()-1; i>=0; i--)
	{
		btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];
		btRigidBody* body = btRigidBody::upcast(obj);
		if (body && body->getMotionState())
		{
			delete body->getMotionState();
		}
		m_dynamicsWorld->removeCollisionObject(obj);
		delete obj;	
	}
	for (int i=0; i<m_collisionShapes.size(); i++)
	{
		btCollisionShape* shape = m_collisionShapes[i];
		m_collisionShapes[i] = 0;
		delete shape;
	}
	delete m_dynamicsWorld;
	delete m_solver;
	delete m_dispatcher;
	delete m_overlappingPairCache;
	delete m_collisionConfiguration;
	m_collisionShapes.clear();
}

void GameMode1Scene::buildBoundingBox( float xmin, float xmax, float ymin, float ymax, float zmin, float zmax )
{
	Ogre::Entity *ent;
	Ogre::Plane p;
	p.normal = Ogre::Vector3(0,1,0); 
	p.d = 0;
	Ogre::MeshManager::getSingleton().createPlane("FloorPlane", 
		Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, 
		p, 200000, 200000, 20, 20, true, 1, 9000, 9000, 
		Ogre::Vector3::UNIT_Z);
	// Create an entity (the floor)
	ent = mSceneMgr->createEntity("floor", "FloorPlane");
	ent->setMaterialName("Examples/BeachStones");
	Ogre::SceneNode *node = mSceneMgr->getRootSceneNode()->createChildSceneNode();
	node->attachObject(ent);
	node->setPosition(0,ymin,0);
	PlaneNode = node;
	//in physics
	{
		//ymin
		btCollisionShape* boundShape = new btStaticPlaneShape(btVector3(0,1,0),0);
		btDefaultMotionState* boundMotionState = new btDefaultMotionState(btTransform(btQuaternion(0,0,0,1),btVector3(0,ymin,0)));
		btRigidBody::btRigidBodyConstructionInfo boundRigidBodyCI(0,boundMotionState,boundShape);
		btRigidBody* boundRigidBody = new btRigidBody(boundRigidBodyCI);
		m_dynamicsWorld->addRigidBody(boundRigidBody, COL_BOUND, boundCollidesWith);
	}
	{
		//ymax
		btCollisionShape* boundShape = new btStaticPlaneShape(btVector3(0,-1,0),0);
		btDefaultMotionState* boundMotionState = new btDefaultMotionState(btTransform(btQuaternion(0,0,0,1),btVector3(0,ymax,0)));
		btRigidBody::btRigidBodyConstructionInfo boundRigidBodyCI(0,boundMotionState,boundShape);
		btRigidBody* boundRigidBody = new btRigidBody(boundRigidBodyCI);
		m_dynamicsWorld->addRigidBody(boundRigidBody, COL_BOUND, boundCollidesWith);
	}
	{
		//xmin
		btCollisionShape* boundShape = new btStaticPlaneShape(btVector3(1,0,0),0);
		btDefaultMotionState* boundMotionState = new btDefaultMotionState(btTransform(btQuaternion(0,0,0,1),btVector3(xmin,0,0)));
		btRigidBody::btRigidBodyConstructionInfo boundRigidBodyCI(0,boundMotionState,boundShape);
		btRigidBody* boundRigidBody = new btRigidBody(boundRigidBodyCI);
		m_dynamicsWorld->addRigidBody(boundRigidBody, COL_BOUND, boundCollidesWith);
	}
	{
		//xmax
		btCollisionShape* boundShape = new btStaticPlaneShape(btVector3(-1,0,0),0);
		btDefaultMotionState* boundMotionState = new btDefaultMotionState(btTransform(btQuaternion(0,0,0,1),btVector3(xmax,0,0)));
		btRigidBody::btRigidBodyConstructionInfo boundRigidBodyCI(0,boundMotionState,boundShape);
		btRigidBody* boundRigidBody = new btRigidBody(boundRigidBodyCI);
		m_dynamicsWorld->addRigidBody(boundRigidBody, COL_BOUND, boundCollidesWith);
	}
	{
		//zmin
		btCollisionShape* boundShape = new btStaticPlaneShape(btVector3(0,0,1),0);
		btDefaultMotionState* boundMotionState = new btDefaultMotionState(btTransform(btQuaternion(0,0,0,1),btVector3(0,0,zmin)));
		btRigidBody::btRigidBodyConstructionInfo boundRigidBodyCI(0,boundMotionState,boundShape);
		btRigidBody* boundRigidBody = new btRigidBody(boundRigidBodyCI);
		m_dynamicsWorld->addRigidBody(boundRigidBody, COL_BOUND, boundCollidesWith);
	}
	{
		//zmax
		btCollisionShape* boundShape = new btStaticPlaneShape(btVector3(0,0,-1),0);
		btDefaultMotionState* boundMotionState = new btDefaultMotionState(btTransform(btQuaternion(0,0,0,1),btVector3(0,0,zmax)));
		btRigidBody::btRigidBodyConstructionInfo boundRigidBodyCI(0,boundMotionState,boundShape);
		btRigidBody* boundRigidBody = new btRigidBody(boundRigidBodyCI);
		m_dynamicsWorld->addRigidBody(boundRigidBody, COL_BOUND, boundCollidesWith);
	}
}

void GameMode1Scene::createScene( void ) 
{
	initPhysics();

	buildBoundingBox(-500,500,-5,1000,-500,500);

	KinectMan = new BodyPartInfo[PARTS_COUNT];
	buildBodyParts();

	int boxShapeCount = 5;
	boxShapes = new BoxShape[boxShapeCount];
	for (int i=0;i<3;i++)
	{
		boxShapes[i].length = btVector3(0.1*i,0.1*i,0.1*i);
		boxShapes[i].shape = CreateSquareShape(&boxShapes[i].length);
	}
	boxShapes[3].length = btVector3(1,1,1);
	boxShapes[3].shape = CreateSquareShape(&boxShapes[3].length);
	boxShapes[4].shape = CreateBallShape(1);
	//buildConstraints();

	// Set ambient light
	mSceneMgr->setAmbientLight(Ogre::ColourValue(0.5, 0.5, 0.5));

	// Create a light
	Ogre::Light* l = mSceneMgr->createLight("MainLight");
	l->setPosition(20,80,50);


	//ShootBox(Ogre::Vector3(10,50,10),Ogre::Vector3(0,-5,0),3);

	// Position it at 500 in Z direction
	mCamera->setPosition(Ogre::Vector3(0,20,50));
	// Look back along -Z
	mCamera->lookAt(Ogre::Vector3(0,0,-300));

	mCameraMan = new OgreBites::SdkCameraMan(mCamera);   // create a default camera controller
	if( !Kinect::getSingleton().isDown ) {
		PRINT("Hi, Kinect!");
		pCharactor = new CharacterController(mCamera);
	} else {
		pCharactor = new PCCharacterController(mCamera);
	}
	//Ogre::Entity *testEntity =  mSceneMgr->getEntity("SinbadBody")->getSkeleton()->getBone("Humerus.L")->
	//mSceneMgr->getSceneNode("Humerus.L")->attachObject(testEntity);
}

void GameMode1Scene::ShootBoxRamdonly()
{
	Ogre::Vector3 randomPos(rand()%10-5, rand()%1000*0.001*3-1.5, rand()%50+50);
	Ogre::Vector3 randomVel(0, 0, -(rand()%100));
	ShootBox(randomPos, randomVel, 1);
}

bool GameMode1Scene::keyPressed( const OIS::KeyEvent &arg )
{
	if (arg.key == OIS::KC_B)
	{
		Ogre::Vector3 randomPos(100, 3, 0);
		Ogre::Vector3 randomVel(-(rand()%100), 0, 0);
		ShootBox(randomPos, randomVel, 3);
	}
	return Scene::keyPressed(arg);
}

bool GameMode1Scene::frameStarted(const Ogre::FrameEvent& evt)
{
	bool ret = FrameListener::frameStarted(evt);

	//UpdatePhysics(evt.timeSinceLastFrame); // update Bullet Physics animation

	return ret;
}

bool GameMode1Scene::frameRenderingQueued(const Ogre::FrameEvent& evt)
{
	pCharactor->addTime(evt.timeSinceLastFrame);
	UpdatePhysics(evt.timeSinceLastFrame); // update Bullet Physics animation
	return Scene::frameRenderingQueued(evt);
}

bool GameMode1Scene::frameEnded(const Ogre::FrameEvent& evt)
{
	bool ret = FrameListener::frameStarted(evt);

	//UpdatePhysics(evt.timeSinceLastFrame); // update Bullet Physics animation

	return ret;
}

void GameMode1Scene::UpdatePhysics(float etime)
{
	//int t=nowFrame/100;
	//if (t<totalFrame)
	//{
	//	for (int j=0;j<PARTS_COUNT;j++)
	//	{
	//		KinectMan[j].Node->setPosition(bodyTrans[t].pos[j]);
	//		KinectMan[j].Node->setOrientation(bodyInit.ort[j]*bodyTrans[t].ort[j]);
	//	}
	//	nowFrame++;
	//}
	BoneData *bones = pCharactor->getBoneData();
	UpdateKinectMan(bones);
	m_dynamicsWorld->stepSimulation(etime);
	DeleteObject();
	toBeDeleted.clear();
	//btCollisionObjectArray bodies = m_dynamicsWorld->getCollisionObjectArray();
	//btRigidBody *TObject;
	//for (int i=0;i<bodies.size();i++)
	//{
	//	TObject = (btRigidBody *)bodies[i];
	//	NodeInfo* nodeInfo = static_cast<NodeInfo*>(TObject->getUserPointer());
	//	if (TObject->isStaticOrKinematicObject())
	//	{
	//		continue;
	//	}
	//}
}

void GameMode1Scene::UpdateKinectMan( BoneData *bones )
{
	for (int j=0;j<PARTS_COUNT;j++)
	{
		KinectMan[j].Node->setPosition(bones[j].BonePosition);
		Ogre::Vector3 vec = Ogre::Vector3::UNIT_Y.crossProduct(bones[j].BoneVector);
		vec.normalise();
		Ogre::Real cosa = bones[j].BoneVector.y/bones[j].BoneVector.length();
		Ogre::Real cosa2 = sqrt((cosa+1)/2);
		Ogre::Real sina2 = sqrt((1-cosa)/2);
		Ogre::Quaternion ort(cosa2,sina2*vec.x,sina2*vec.y,sina2*vec.z);
		//Ogre::Quaternion ort = bones[j].InitialOrientation*bones[j].BoneOrientation;
		//Ogre::Quaternion ort = bones[j].BoneOrientation;
		//Ogre::Quaternion ort = bones[j].BoneOrientation*bones[j].InitialOrientation;
		KinectMan[j].Node->setOrientation(ort);
		btTransform trans;
		trans.setIdentity();
		trans.setOrigin(btVector3(bones[j].BonePosition.x,bones[j].BonePosition.y,bones[j].BonePosition.z));
		trans.setRotation(btQuaternion(ort.x,ort.y,ort.z,ort.w));
		KinectMan[j].Body->setWorldTransform(trans);
	}
}

void GameMode1Scene::DeleteObject()
{
	btRigidBody* node;
	for (std::set<btRigidBody*>::iterator it=toBeDeleted.begin();it!=toBeDeleted.end();it++)
	{
		node = *it;
		PapaNode* nodeInfo = static_cast<PapaNode*>(node->getUserPointer());
		mSceneMgr->destroySceneNode(nodeInfo->getNode());
		m_dynamicsWorld->removeCollisionObject(node);
		PRINT(nodeInfo->name+" deleted!");
		delete nodeInfo;
	}
}

void GameMode1Scene::buildBodyParts()
{
	btVector3 WaistLength(1,0.6685,0.797);
	btVector3 ChestLength(1.251,1,1.041);
	btVector3 HeadLength(0.753,0.75,0.6);
	btVector3 ArmULength(0.639091,0.925414,0.639091);
	btVector3 ArmLLength(0.471628,0.767424,0.471628);
	btVector3 LegULength(0.5,0.819411,0.5);
	btVector3 LegLLength(0.35,0.90739,0.35);

	bool useKMS = true;
	BodyPartInfo *part;
	float mass = 0;
	//Body parts
	//Waist
	part = &(KinectMan[WAIST]);
	part->Length = WaistLength;
	part->Center.setIdentity();
	//Chest
	part = &(KinectMan[CHEST]);
	part->Length = ChestLength;
	part->Center.setIdentity();
	part->Center.setOrigin(btVector3(0,KinectMan[WAIST].Center.getOrigin().y()+KinectMan[WAIST].Length.y()+KinectMan[CHEST].Length.y(),0));
	//Head
	part = &(KinectMan[HEAD]);
	part->Length = HeadLength;
	part->Center.setOrigin(btVector3(0,KinectMan[CHEST].Center.getOrigin().y()+KinectMan[CHEST].Length.y()+KinectMan[HEAD].Length.y(),0));
	//UpperArmLeft
	part = &(KinectMan[ARMUL]);
	part->Length = ArmULength;
	part->Center.setIdentity();
	part->Center.setOrigin(btVector3(KinectMan[CHEST].Center.getOrigin().x()+KinectMan[CHEST].Length.x()+ArmULength.x(),KinectMan[CHEST].Center.getOrigin().y()+KinectMan[CHEST].Length.y()-KinectMan[ARMUL].Length.y(),0));
	//LowerArmLeft
	part = &(KinectMan[ARMLL]);
	part->Length = ArmLLength;
	part->Center.setIdentity();
	part->Center.setOrigin(btVector3(KinectMan[ARMUL].Center.getOrigin().x()+KinectMan[ARMUL].Length.x()+KinectMan[ARMLL].Length.x(),KinectMan[ARMUL].Center.getOrigin().y(),0));
	//UpperLegLeft
	part = &(KinectMan[LEGUL]);
	part->Length = LegULength;
	part->Center.setIdentity();
	part->Center.setOrigin(btVector3(KinectMan[WAIST].Center.getOrigin().x()+KinectMan[WAIST].Length.x()-KinectMan[LEGUL].Length.x(),KinectMan[WAIST].Center.getOrigin().y()-KinectMan[WAIST].Length.y()-KinectMan[LEGUL].Length.y(),0));
	//LowerLegLeft
	part = &(KinectMan[LEGLL]);
	part->Length = LegLLength;
	part->Center.setIdentity();
	part->Center.setOrigin(btVector3(KinectMan[LEGUL].Center.getOrigin().x(),KinectMan[LEGUL].Center.getOrigin().y()-KinectMan[LEGUL].Length.y()-KinectMan[LEGLL].Length.y(),0));
	//UpperArmRight
	part = &(KinectMan[ARMUR]);
	part->Length = ArmULength;
	part->Center.setIdentity();
	part->Center.setOrigin(btVector3(-KinectMan[ARMUL].Center.getOrigin().x(),KinectMan[ARMUL].Center.getOrigin().y(),0));
	//LowerArmRight
	part = &(KinectMan[ARMLR]);
	part->Length = ArmLLength;
	part->Center.setIdentity();
	part->Center.setOrigin(btVector3(-KinectMan[ARMLL].Center.getOrigin().x(),KinectMan[ARMLL].Center.getOrigin().y(),0));
	//UpperLegRight
	part = &(KinectMan[LEGUR]);
	part->Length = LegULength;
	part->Center.setIdentity();
	part->Center.setOrigin(btVector3(-KinectMan[LEGUL].Center.getOrigin().x(),KinectMan[LEGUL].Center.getOrigin().y(),0));
	//LowerLegRight
	part = &(KinectMan[LEGLR]);
	part->Length = LegLLength;
	part->Center.setIdentity();
	part->Center.setOrigin(btVector3(-KinectMan[LEGLL].Center.getOrigin().x(),KinectMan[LEGLL].Center.getOrigin().y(),0));

	for (int i=0;i<PARTS_COUNT;i++)
	{
		part = &(KinectMan[i]);
		part->Shape =  CreateSquareShape(&part->Length);
		part->Node = AddOgreBox(&part->Length, Ogre::Vector3(part->Center.getOrigin().x(),part->Center.getOrigin().y(),part->Center.getOrigin().z()), BodyPartName[i]);
		part->Body = CreateRigidBody(mass,&part->Center,part->Shape,COL_BODY,bodyCollidesWith,part->Node,useKMS);
		PapaNode *nodeInfo = new PapaNode(part->Node, BodyPartName[i], "man", part->Body);
		part->Body->setUserPointer((void*)nodeInfo);
	}

}

void GameMode1Scene::buildConstraints()
{
	//btVector3 WaistLength(1,0.6685,0.797);
	//btVector3 ChestLength(1.251,1,1.041);
	//btVector3 HeadLength(0.753,0.75,0.6);
	//btVector3 ArmULength(0.925414,0.639091,0.639091);
	//btVector3 ArmLLength(0.767424,0.471628,0.471628);
	//btVector3 LegULength(0.5,0.819411,0.5);
	//btVector3 LegLLength(0.35,0.90739,0.35);

	////Constraints
	//btVector3 pivotInA(0,WaistLength.y(),0);
	//btVector3 pivotInB(0,-ChestLength.y(),0);
	////Waist-Chest
	//pivotInA = btVector3(0,WaistLength.y(),0);
	//pivotInB = btVector3(0,-ChestLength.y(),0);
	//m_dynamicsWorld->addConstraint(CreatePoint2PointConstraint(Waist, Chest, &pivotInA, &pivotInB), true);
	////Chest-Head
	//pivotInA = btVector3(0,ChestLength.y(),0);
	//pivotInB = btVector3(0,-HeadLength.y(),0);
	//m_dynamicsWorld->addConstraint(CreatePoint2PointConstraint(Chest, Head, &pivotInA, &pivotInB), true);
	////Chest-UpperArmLeft
	//pivotInA = btVector3(ChestLength.x(),ChestLength.y()-ArmULength.y(),0);
	//pivotInB = btVector3(-ArmULength.x(),0,0);
	//m_dynamicsWorld->addConstraint(CreatePoint2PointConstraint(Chest, ArmUL, &pivotInA, &pivotInB), true);
	////UpperArmLeft-LowerArmRight
	//pivotInA = btVector3(ArmULength.x(),0,0);
	//pivotInB = btVector3(-ArmLLength.x(),0,0);
	//m_dynamicsWorld->addConstraint(CreatePoint2PointConstraint(ArmUL, ArmLL, &pivotInA, &pivotInB), true);
	////Waist-UpperLegLeft
	//pivotInA = btVector3(WaistLength.x()-LegULength.x(),-WaistLength.y(),0);
	//pivotInB = btVector3(0,LegULength.y(),0);
	//m_dynamicsWorld->addConstraint(CreatePoint2PointConstraint(Waist, LegUL, &pivotInA, &pivotInB), true);
	////UpperLegLeft-LowerLegLeft
	//pivotInA = btVector3(0,-LegULength.y(),0);
	//pivotInB = btVector3(0,LegLLength.y(),0);
	//m_dynamicsWorld->addConstraint(CreatePoint2PointConstraint(LegUL, LegLL, &pivotInA, &pivotInB), true);
	////Chest-UpperArmRight
	//pivotInA = btVector3(-ChestLength.x(),ChestLength.y()-ArmULength.y(),0);
	//pivotInB = btVector3(ArmULength.x(),0,0);
	//m_dynamicsWorld->addConstraint(CreatePoint2PointConstraint(Chest, ArmUR, &pivotInA, &pivotInB), true);
	////UpperArmRight-LowerArmRight
	//pivotInA = btVector3(-ArmULength.x(),0,0);
	//pivotInB = btVector3(ArmLLength.x(),0,0);
	//m_dynamicsWorld->addConstraint(CreatePoint2PointConstraint(ArmUR, ArmLR, &pivotInA, &pivotInB), true);
	////Waist-UpperLegRight
	//pivotInA = btVector3(-WaistLength.x()+LegULength.x(),-WaistLength.y(),0);
	//pivotInB = btVector3(0,LegULength.y(),0);
	//m_dynamicsWorld->addConstraint(CreatePoint2PointConstraint(Waist, LegUR, &pivotInA, &pivotInB), true);
	////UpperLegRight-LowerLegRight
	//pivotInA = btVector3(0,-LegULength.y(),0);
	//pivotInB = btVector3(0,LegLLength.y(),0);
	//m_dynamicsWorld->addConstraint(CreatePoint2PointConstraint(LegUR, LegLR, &pivotInA, &pivotInB), true);
}

btTypedConstraint* GameMode1Scene::CreatePoint2PointConstraint(btRigidBody *a_body,btRigidBody *b_body, btVector3 *pivotPosInA, btVector3 *pivotPosInB)
{		

	btPoint2PointConstraint *pointC = new btPoint2PointConstraint(*a_body, *b_body, *pivotPosInA, *pivotPosInB);

	m_constraints.push_back(pointC);

	return pointC;

}

btRigidBody* GameMode1Scene::CreateRigidBody(float mass, btTransform *trans, btCollisionShape *a_shape, short group, short mask, Ogre::SceneNode *node, bool useKMS)
{
	bool isDynamic = (mass != 0.f);

	btVector3 localInertia(0,0,0);
	if (isDynamic)
		a_shape->calculateLocalInertia(mass,localInertia);

	btDefaultMotionState *myMotionState;

	if (useKMS)
	{
		myMotionState = (btDefaultMotionState *)new MyKinematicMotionState(*trans);
	} 
	else
	{
		myMotionState = (btDefaultMotionState *)new MyMotionState(*trans, node);
	}

	btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,a_shape,localInertia);
	btRigidBody* body = new btRigidBody(rbInfo);

	m_dynamicsWorld->addRigidBody(body, group, mask);

	return body;
}

btCollisionShape* GameMode1Scene::CreateSquareShape(btVector3 *vec)
{
	btBoxShape *t_shape = new btBoxShape(*vec);	
	m_collisionShapes.push_back(t_shape);
	return t_shape;
}

btCollisionShape* GameMode1Scene::CreateBallShape( btScalar r )
{
	btSphereShape *t_shape = new btSphereShape(r);
	m_collisionShapes.push_back(t_shape);
	return t_shape;
}

Ogre::SceneNode* GameMode1Scene::AddOgreBox( btVector3 *length, Ogre::Vector3 position, Ogre::String name )
{
	Ogre::Entity *entity = mSceneMgr->createEntity(name, "cube.mesh");			    
	entity->setCastShadows(true);
	entity->setMaterialName("Examples/BumpyMetal");
	Ogre::SceneNode *node = mSceneMgr->getRootSceneNode()->createChildSceneNode();
	node->attachObject(entity);
	btScalar t = 0.02;
	node->scale(t*length->x(), t*length->y(), t*length->z());
	node->setPosition(position);
	return node;
}

Ogre::SceneNode* GameMode1Scene::AddOgreBall( btScalar r, Ogre::Vector3 position, Ogre::String name )
{
	Ogre::Entity *entity = mSceneMgr->createEntity(name, "sphere.mesh");			    
	entity->setCastShadows(true);
	entity->setMaterialName("Examples/BumpyMetal");
	Ogre::SceneNode *node = mSceneMgr->getRootSceneNode()->createChildSceneNode();
	node->attachObject(entity);
	btScalar t = 0.02;
	node->scale(t*r,t*r,t*r);
	node->setPosition(position);
	return node;
}

void GameMode1Scene::readFile( std::string inputfile )
{
	std::ifstream in(inputfile.c_str());
	if (in.is_open())
	{
		in>>totalFrame;
		totalFrame/=PARTS_COUNT;
		bodyTrans = new BodyTransform[totalFrame];
		double x,y,z,w;
		for (int i=0;i<totalFrame;i++)
		{
			for (int j=0;j<PARTS_COUNT;j++)
			{
				in>>x>>y>>z;
				bodyTrans[i].pos[j]=Ogre::Vector3(x,y,z);
				in>>w>>x>>y>>z;
				bodyTrans[i].ort[j]=Ogre::Quaternion(w,x,y,z);
			}
		}
	}
	in.close();
	in.open("D:\\IntialOrientation.txt");
	if (in.is_open())
	{
		double x,y,z,w;
		for (int j=0;j<PARTS_COUNT;j++)
		{
			in>>x>>y>>z;
			bodyInit.pos[j]=Ogre::Vector3(x,y,z);
			in>>w>>x>>y>>z;
			bodyInit.ort[j]=Ogre::Quaternion(w,x,y,z);
		}
	}
	in.close();
	nowFrame=0;
}

void GameMode1Scene::ShootBox( Ogre::Vector3 start, Ogre::Vector3 linearVelocity, int l )
{
	PRINT("shooting Box"+Ogre::StringConverter::toString(mNumEntitiesInstanced));
	Ogre::SceneNode *node = AddOgreBox(&boxShapes[l].length,start,"Box"+Ogre::StringConverter::toString(mNumEntitiesInstanced));
	btTransform trans;
	trans.setIdentity();
	trans.setOrigin(Vector3OgreToBullet(start));
	trans.setRotation(btQuaternion(0,0,0,1));
	btRigidBody* boxBody = CreateRigidBody(1,&trans,boxShapes[l].shape,COL_AIRBOX,airboxCollidesWith,node);
	boxBody->setLinearVelocity(Vector3OgreToBullet(linearVelocity));
	PapaNode *nodeInfo = new PapaNode(node, "Box"+Ogre::StringConverter::toString(mNumEntitiesInstanced), "box", boxBody);
	boxBody->setUserPointer((void*)nodeInfo);
	PRINT("Box"+Ogre::StringConverter::toString(mNumEntitiesInstanced)+" shooted");
	mNumEntitiesInstanced++;
}

void GameMode1Scene::AddBoundingBoxToPhysics( Ogre::AxisAlignedBox *AABB, bool isStatic, Ogre::SceneNode *node)
{
	btVector3 size = Vector3OgreToBullet(AABB->getHalfSize());
	btTransform trans;
	trans.setIdentity();
	trans.setOrigin(Vector3OgreToBullet(AABB->getCenter()));
	trans.setRotation(btQuaternion(0,0,0,1));
	int mass;
	if (isStatic)
	{
		mass = 0;
	}
	else
	{
		mass = 1;
	}
	CreateRigidBody(mass,&trans,CreateSquareShape(&size),COL_OBSTACLE,obstacleCollidesWith,node,isStatic);
}
